    #include<ros/ros.h> 
    #include<iostream> 
    #include<cv_bridge/cv_bridge.h>   
    #include<sensor_msgs/image_encodings.h>   
    #include<image_transport/image_transport.h>   
    #include<opencv2/core/core.hpp>  
    #include<opencv2/highgui/highgui.hpp>  
    #include<opencv2/imgproc/imgproc.hpp>  
      
      
    class STEREO  
    {  
    private:  
        ros::NodeHandle nh_; 
        image_transport::ImageTransport it_; 
        image_transport::Subscriber left_image_sub_; 
        image_transport::Subscriber right_image_sub_;
	image_transport::Publisher stereo_image_pub_; 
	bool left_recv_,right_recv_;
        cv::VideoWriter writer;
    
        
    public:  
        STEREO()  
          :it_(nh_) //构造函数  
        {  
            left_image_sub_ = it_.subscribe("/left_camera/image_raw", 1, &STEREO::left_image_callback, this);
            right_image_sub_ = it_.subscribe("/right_camera/image_raw", 1, &STEREO::right_image_callback, this);
	    stereo_image_pub_ = it_.advertise("/stereo_image", 1);  
	    left_recv_ = false;
	    right_recv_ = false;
	    cv::namedWindow("stereo", 0);
 	    writer.open("/home/allen/test.avi", CV_FOURCC('M', 'J', 'P', 'G'), 30, cv::Size(2560, 720), true);
        }  
        ~STEREO() //析构函数  
        {  
            
        }  
	cv_bridge::CvImagePtr left_cv_ptr_,right_cv_ptr_,stereo_cv_ptr_;
	cv::Mat stereo_mat_;
	sensor_msgs::ImagePtr stereo_image_ptr_;
        void left_image_callback(const sensor_msgs::ImageConstPtr& msg)   
        {  
	    try  
            {  
                left_cv_ptr_ =  cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); 
            }  
            catch(cv_bridge::Exception& e)  //异常处理  
            {  
                ROS_ERROR("cv_bridge exception: %s", e.what());  
                return;  
            }  
            left_recv_=true;
        }  
	void right_image_callback(const sensor_msgs::ImageConstPtr& msg)   
        {  
	    try  
            {  
                right_cv_ptr_ =  cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); 
            }  
            catch(cv_bridge::Exception& e)  //异常处理  
            {  
                ROS_ERROR("cv_bridge exception: %s", e.what());  
                return;  
            }  
	    image_process(left_cv_ptr_->image , right_cv_ptr_->image);
	    

        }  
	void image_process(cv::Mat left_img , cv::Mat right_img)   
        {  
	      
	      if(left_recv_)
	      {
		cv::Mat img_merge;
  		cv::Size size(left_img.cols + right_img.cols, MAX(left_img.rows, right_img.rows));
    		img_merge.create(size, CV_MAKETYPE(right_img.depth(), 3));
    		img_merge = cv::Scalar::all(0);
    		cv::Mat outImg_left, outImg_right;
    		//2.在新建合并图像中设置感兴趣区域
   		outImg_left = img_merge(cv::Rect(0, 0, left_img.cols, left_img.rows));
    		outImg_right = img_merge(cv::Rect(left_img.cols, 0, left_img.cols, left_img.rows));
    		//3.将待拷贝图像拷贝到感性趣区域中
    		left_img.copyTo(outImg_left);
    		right_img.copyTo(outImg_right);
		//stereo_image_ptr_ = cv_bridge::CvImage(std_msgs::Header(), "bgr8", img_merge).toImageMsg();  
		//stereo_image_pub_.publish(stereo_image_ptr_);
  		cv::imshow("stereo", img_merge);
		writer << img_merge;
		cv::waitKey(1);
		left_recv_ = false;
	      }
        }  
    };  
      
    //主函数  
    int main(int argc, char** argv)  
    {  
        ros::init(argc, argv, "stereo_image");  
        STEREO obj;  
        ros::spin();  
    }  
